#include "CubePile.h"
#include "Constants.h"
/*
#include <iostream>
using namespace std;
 */
CubePile::CubePile(Cube * bottomCube, Sound * aSound, float posX, float posZ, CubeBounceFinishedEventListener * aListener)
{
	sound = aSound;
	stationaryCubes.push_back(bottomCube);
	xPos = posX;
	zPos = posZ;
	bottomCube->xPos = xPos;
	bottomCube->zPos = zPos;
	listener = aListener;
	locked = false;
	height = bottomCube->height;
	feature = NULL;
}

CubePile::~CubePile()
{
	//TODO: delete cubes
}

bool CubePile::canDropCube()
{
	return size() < MAX_CUBES_IN_PILE && !locked;
}

void CubePile::lock()
{
	locked = true;
}

void CubePile::setFeature(CubePileFeature * aFeature)
{
	feature = aFeature;
}

CubePileFeature * CubePile::getFeature()
{
	return feature;
}

void CubePile::dropCube(Cube* cube)
{
	cube->xPos = xPos;
	cube->zPos = zPos;
	cube->yVel = 0;
	bouncingCubes.push_back(cube);
}

Cube * CubePile::peekAtTop()
{
	if(bouncingCubes.size() > 0)
	{
		return bouncingCubes.at(bouncingCubes.size() -1);
	}
	else
	{
		return stationaryCubes.at(stationaryCubes.size() -1);
	}
}

Cube* CubePile::popCube()
{
	if(bouncingCubes.size() >0)
	{
		Cube* ret = bouncingCubes.at(bouncingCubes.size() -1);
		bouncingCubes.pop_back();
		return ret;
	}
	else if(stationaryCubes.size() > 1)
	{
		Cube* ret = stationaryCubes.at(stationaryCubes.size() -1);
		stationaryCubes.pop_back();
		height -= ret->height;
		return ret;
	}
	else
	{
		return NULL;
	}
}

void CubePile::testAndHandleCollisions(float timeD)
{
	bool collision = true;
	int cycles = 0;
    while (collision && cycles++ < 100)
    {
		collision = false;
		
		if(bouncingCubes.size() > 0)
		{
			//test for collision with floor or top stationary cube
			Cube * bottomBouncer = bouncingCubes.at(0);
			Cube * topStatic = stationaryCubes.at(stationaryCubes.size()-1);
			if(bottomBouncer -> yPos < height)
			{
				bottomBouncer->yVel *=-1 * GROUND_BOUNCE_LOSS;
				raiseAOverB(bottomBouncer, topStatic);
    			sound->playImpactSound(bottomBouncer->yVel * bottomBouncer->yVel);
			}

			//Test for collision with other bouncing cubes
	        for (int i=1; (unsigned)i<bouncingCubes.size(); i++)
	        {
	            Cube * a = bouncingCubes.at(i);
	            Cube * b = bouncingCubes.at(i-1);
	            if (isCollide(a->yPos, b->yPos, a->height))
	            {
	                collision = true;
	                handleCollision(a, b, timeD);
	            }
	        }		
        }
    }
}

void CubePile::animate(float timeD)
{
	for(int i=0; (unsigned)i<bouncingCubes.size(); i++)
	{
		bouncingCubes.at(i)->animate(timeD);
	}
	
	testAndHandleCollisions(timeD);
	
	moveStillCubesFromBounceList();
}

void CubePile::moveStillCubesFromBounceList()
{
	bool keep = false;
	while(!keep)
	{
		keep = true;
		if(bouncingCubes.size() > 0)
		{
			Cube * bottomBouncer = bouncingCubes.at(0);
			if(bottomBouncer->yPos < height + bottomBouncer->height)
			{
//				cout << "in remove check zone\n";
				float yVel = bottomBouncer->yVel;
				if (yVel < 0)
				{
					yVel *=-1;
				}
				if( yVel < MIN_CUBE_SPEED)
				{
//					cout << "removing from list\n";
					bouncingCubes.erase(bouncingCubes.begin());//remove first element
					bottomBouncer->yVel = 0;
					bottomBouncer->yPos = height + bottomBouncer->height - stationaryCubes.at(0)->height;
					height += bottomBouncer->height;
					stationaryCubes.push_back(bottomBouncer);
					if(bouncingCubes.size() == 0 && listener != NULL)
					{
//						cout << "empty bouncer list\n";
						listener->cubeBounceFinished((int)(xPos/CUBE_SPACING), (int)(zPos/CUBE_SPACING));
					}
					keep = false;
				}
			}
		}
	}
}

bool CubePile::isBouncing()
{
	return bouncingCubes.size() > 0;
}

bool CubePile::isCollide(float aPos, float bPos, float size) {
	bool collision = aPos > bPos and aPos < bPos + size;
	return collision;
}

void CubePile::swapVelocities(Cube * a, Cube * b)
{
    float bVel = a->yVel;
    a->yVel = b->yVel * CUBE_BOUNCE_LOSS;
    b->yVel = bVel * CUBE_BOUNCE_LOSS;
}

void CubePile::raiseHigher(Cube * a, Cube * b)
{
    if (a->yPos > b->yPos)
    {
        raiseAOverB(a, b);
    }
    else
    {
        raiseAOverB(b, a);
    }
}

void CubePile::raiseAOverB(Cube * a, Cube * b)
{
	float difference = a->yPos - b->yPos;
	a->yPos = a->yPos + a->height - difference;
}

void CubePile::handleCollision(Cube * a, Cube * b, float timeD)
{
    swapVelocities(a,b);
    raiseHigher(a,b);
    
    //squaring to remove -ve (i know, super dodge)
    sound->playImpactSound(a->yVel * a->yVel + b->yVel * b->yVel);
}

void CubePile::render()
{
	for(int i=0; (unsigned)i < bouncingCubes.size(); i++)
	{
		bouncingCubes.at(i)->render();
	}
	
	for(int i=0; (unsigned)i < stationaryCubes.size(); i++)
	{
		stationaryCubes.at(i)->render();
	}
	
	if(feature != NULL)
	{
		glPushMatrix();
		glTranslatef(xPos, 0, zPos);
		feature->setYPos(size());
		feature->render();
		glPopMatrix();
	}
}

void CubePile::renderBottom()
{
	stationaryCubes.at(0)->render();	
}

int CubePile::size()
{
	return bouncingCubes.size() + stationaryCubes.size();
}

bool CubePile::isLocked()
{
	return locked;
}
